#include <iostream>
#include <sys/types.h>
#include <sys/socket.h>
#include <sys/select.h>
#include <netinet/in.h>
#include <netdb.h>
#include <stdio.h>
#include <stdlib.h>


#include "modbus.h"

using namespace std;

// Funcoes auxiliares locais

static uint16_t registroInicial(uint8_t funcao)
{
  uint8_t funcao2=funcao;
  if (funcao2 >= 128) funcao2 -= 128;
  switch(funcao2) {
  case 1:
  case 5:
  case 15:
    return 1;      // 0xxx
  case 2:
    return 10001;  // 1xxxx
  case 3:
  case 6:
  case 16:
    return 40001;  // 4xxxx
  case 4:
    return 30001;  // 3xxxx
  }
  return (uint16_t)-1;
}

static MODBUS_STATUS testaFaixaRegistros(uint16_t regNorm, uint16_t numReg)
{
  if (numReg<1 || numReg>9999) {
    cerr << "Numero de registros invalido: " << numReg << endl;
    return MODBUS_ERRO_DADO;
  }
  if (regNorm+numReg > 9999) {
    cerr << "Estouro na faixa de registros: "
	 << regNorm << "," << numReg << endl;
    return MODBUS_ERRO_DADO;
  }
  return MODBUS_OK;
}

static MODBUS_STATUS testaIdPoco(uint8_t idPoco)
{
  if (idPoco<1 || idPoco>247) {
    cerr << "Id de poco invalida: " << (unsigned)idPoco << endl;
    return MODBUS_ERRO_DADO;
  }
  return MODBUS_OK;
}

static MODBUS_STATUS testaDados(const uint8_t *dados)
{
  if (dados == NULL) {
    cerr << "Dados invalidos\n";
    return MODBUS_ERRO_DADO;
  }
  return MODBUS_OK;
}

static MODBUS_STATUS converteRegistros(uint16_t registro, uint16_t numReg,
				       bool leitura,
				       uint16_t &regNorm, uint8_t &funcao,
				       int16_t &lenDados)
{
  if (registro == 0 || registro >= 50000) {
    cerr << "Endereco de registro invalido: " << registro << endl;
    return MODBUS_ERRO_DADO;
  }
  else if (registro <= 10000) {
    // 0xxxx
    funcao = (leitura ? 1 : (numReg==1 ? 5 : 15));
    regNorm = registro-1;
    lenDados = numReg;
  }
  else if (registro <= 20000) {
    // 1xxxx
    if (leitura) {
      funcao = 2;
      regNorm = registro-10001;
      lenDados = numReg;
    }
    else {
      cerr << "Registro apenas de leitura: " << registro << endl;
      return MODBUS_ERRO_DADO;
    }
  }
  else if (registro <= 30000) {
    // 2xxxx
    cerr << "Endereco de registro invalido: " << registro << endl;
    return MODBUS_ERRO_DADO;
  }
  else if (registro <= 40000) {
    // 3xxxx
    if (leitura) {
      funcao = 4;
      regNorm = registro-30001;
      lenDados = 2*numReg;
    }
    else {
      cerr << "Registro apenas de leitura: " << registro << endl;
      return MODBUS_ERRO_DADO;
    }
  }
  else if (registro <= 50000) {
    // 4xxxx
    funcao = (leitura ? 3 : (numReg==1 ? 6 : 16));
    regNorm = registro-40001;
    lenDados = 2*numReg;
  }
  return testaFaixaRegistros(regNorm,numReg);
}

/* *******************************************
   A classe Mobus
   ******************************************* */

// Construtores e destrutores
Modbus::Modbus(): sockId(-1) {}

Modbus::~Modbus()
{
  ::close(sockId);
}

// Funcoes que nunca devem ser chamadas...
Modbus::Modbus(const Modbus&)
{
  cerr << "Nunca use o construtor por copia da classe Modbus\n";
  exit(1);
}

void Modbus::operator=(const Modbus&)
{
  cerr << "Nunca use o operador de atribuicao da classe Modbus\n";
  exit(1);
}

// Funcoes privadas

ssize_t Modbus::myRecv(int timeout, void *buf, size_t len, int flags)
{
  static fd_set filaSockets;
  struct timeval tmout;
  if (timeout>=0 && sockId>=0) {
    // Preenche a fila para select
    FD_ZERO(&filaSockets);
    FD_SET(sockId, &filaSockets);
    // Calcula o timeout em segundos e microssegundos
    tmout.tv_sec=timeout/1000;
    tmout.tv_usec=timeout-1000*tmout.tv_sec;
    // Espera que haja dados para leitura disponíveis
    int status = select(sockId+1, &filaSockets, NULL, NULL, &tmout);
    if (status < 0) {
      cerr << "Erro na espera por dados para leitura\n";
      return -1;
    }
    if (status == 0) {
      return 0;
    }
  }
  // Le os dados
  return virtRecv(buf, len, flags);
}

void Modbus::limpaSocket()
{
  uint8_t lixo;
  while (virtRecv(&lixo, 1, MSG_DONTWAIT) == 1);
}

MODBUS_STATUS Modbus::leIdPoco(int timeout, uint8_t &idPoco)
{
  ssize_t numBytes;
  if ((numBytes=myRecv(timeout, &idPoco, 1, MSG_WAITALL)) != 1) {
    if (numBytes==0) return MODBUS_TIMEOUT;
    cerr << "Erro na leitura da id do poco\n";
    idPoco = 0;
    return MODBUS_ERRO_COMUNICACAO;
  }
  return testaIdPoco(idPoco);
}

MODBUS_STATUS Modbus::leFuncao(int timeout, uint8_t &funcao)
{
  ssize_t numBytes;
  if ((numBytes=myRecv(timeout, &funcao, 1, MSG_WAITALL)) != 1) {
    if (numBytes==0) return MODBUS_TIMEOUT;
    cerr << "Erro na leitura da funcao\n";
    return MODBUS_ERRO_COMUNICACAO;
  }
  uint8_t funcao2=funcao;
  if (funcao2 >= 128) funcao2 -= 128;
  if ( funcao2<1 || funcao2>16 || (funcao2>6 && funcao2<15) ) {
    cerr << "Funcao invalida: " << (unsigned)funcao << endl;
    limpaSocket();
    return MODBUS_ERRO_DADO;
  }
  if (funcao >= 128) {
     return MODBUS_RESPOSTA_ERRO;
  }
  return MODBUS_OK;
}

MODBUS_STATUS Modbus::escreveIdFuncao(uint8_t idPoco, uint8_t funcao)
{
  // Poco
  if (send(sockId, &idPoco, 1, MSG_NOSIGNAL) != 1) {
    cerr << "Erro na escrita da id do poco\n";
    return MODBUS_ERRO_COMUNICACAO;
  }
  // Funcao
  if (send(sockId, &funcao, 1, MSG_NOSIGNAL) != 1) {
    cerr << "Erro na escrita da funcao\n";
    return MODBUS_ERRO_COMUNICACAO;
  }
  return MODBUS_OK;
}

MODBUS_STATUS Modbus::escreveDados(const uint8_t *dados, int16_t lenDados)
{
  if (send(sockId, dados, lenDados, MSG_NOSIGNAL) != lenDados) {
    cerr << "Erro na escrita dos dados\n";
    return MODBUS_ERRO_COMUNICACAO;
  }
  return MODBUS_OK;
}

/* *******************************************
   A classe MobusEscravo
   ******************************************* */

// Construtores e destrutores
ModbusEscravo::ModbusEscravo(unsigned porta):
  conectado(false)
{
  if (criaSocket(porta)) {
    cerr << "Nao foi possivel criar e/ou ligar o socket\n";
    exit(1);
  }
}

// Funcoes privadas

ssize_t ModbusEscravo::virtRecv(void *buf, size_t len, int flags)
{
  ssize_t numBytes;
  if (conectado) {
    numBytes = ::recv(sockId, buf, len, flags);
    conectado = (numBytes>=0);
    return numBytes;
  }
  // Recebe a mensagem inicial do mestre
  struct sockaddr_in mestre;
  socklen_t sizebuf=sizeof(struct sockaddr);
  numBytes = recvfrom(sockId, buf, len, flags,
		      (struct sockaddr *)&mestre, &sizebuf);
  if (numBytes > 0) {
    // Conecta-se ao mestre
    if(::connect(sockId,(struct sockaddr*)&mestre,sizeof(mestre)) == -1 ) {
      cerr << "Erro na conexao com o mestre\n";
    }
    else {
      conectado = true;
    }
  }
  return numBytes;
}

MODBUS_STATUS ModbusEscravo::criaSocket(unsigned porta)
{
  if (!conectado) {
    sockId = ::socket(PF_INET, SOCK_DGRAM, 0);
    if (sockId<0) return MODBUS_ERRO_COMUNICACAO;
    
    // Atribuicao do nome do socket
    struct sockaddr_in servidor;
    
    servidor.sin_family = AF_INET;
    servidor.sin_port = htons(porta);
    servidor.sin_addr.s_addr = INADDR_ANY;
    // Aceita escrita de qualquer maquina
    if ( ::bind(sockId,(struct sockaddr *)&servidor,sizeof(servidor)) == -1 )
      return MODBUS_ERRO_COMUNICACAO;
  }
  else {
    cerr << "Socket ja esta aceitando conexoes\n";
    return MODBUS_ERRO_COMUNICACAO;
  }
  return MODBUS_OK;
}

MODBUS_STATUS ModbusEscravo::leRegistroNormalizado(int timeout,
						   uint16_t &regNorm)
{
  ssize_t numBytes;
  if ((numBytes=myRecv(timeout, &regNorm, sizeof(uint16_t),
		       MSG_WAITALL)) != sizeof(uint16_t)) {
    if (numBytes==0) return MODBUS_TIMEOUT;
    cerr << "Erro na leitura do registro normalizado\n";
    regNorm = 0;
    return MODBUS_ERRO_COMUNICACAO;
  }
  return MODBUS_OK;
}

MODBUS_STATUS ModbusEscravo::leNumeroRegistros(int timeout, uint16_t &numReg)
{
  ssize_t numBytes;
  if ((numBytes=myRecv(timeout, &numReg, sizeof(uint16_t),
		       MSG_WAITALL)) != sizeof(uint16_t)) {
    if (numBytes==0) return MODBUS_TIMEOUT;
    cerr << "Erro na leitura do numero de registros\n";
    numReg = 0;
    return MODBUS_ERRO_COMUNICACAO;
  }
  return MODBUS_OK;
}

MODBUS_STATUS ModbusEscravo::leDadosBuffer(int timeout, int16_t lenDados)
{
  ssize_t numBytes;
  if ((numBytes=myRecv(timeout, modBuffer, lenDados,
		       MSG_WAITALL)) != lenDados) {
    if (numBytes==0) return MODBUS_TIMEOUT;
    cerr << "Erro na leitura dos dados\n";
    return MODBUS_ERRO_COMUNICACAO;
  }
  return MODBUS_OK;
}

MODBUS_STATUS ModbusEscravo::escreveTamanho(int16_t lenDados)
{
  if (send(sockId, &lenDados, sizeof(int16_t), MSG_NOSIGNAL) !=
      sizeof(int16_t)) {
    cerr << "Erro na escrita do numero de bytes\n";
    return MODBUS_ERRO_COMUNICACAO;
  }
  return MODBUS_OK;
}

// Funcoes publicas

MODBUS_STATUS ModbusEscravo::aceitarNovaConexao(unsigned porta)
{
  if (conectado) {
    if (::close(sockId) == -1) {
      cerr << "Nao foi possivel fechar o socket\n";
      return MODBUS_ERRO_COMUNICACAO;
    }
    conectado = false;
    if (criaSocket(porta)) {
      cerr << "Nao foi possivel criar e/ou ligar o socket\n";
      return MODBUS_ERRO_COMUNICACAO;
    }
  }
  return MODBUS_OK;
}

// Escravo recebe pedido de leitura ou escrita do mestre
MODBUS_STATUS ModbusEscravo::read_request(int timeout,
					  uint8_t &idPoco, bool &leitura,
					  uint16_t &registro, uint16_t &numReg)
{
  // Valores para o caso de retornar por erro
  idPoco = 0;
  leitura = true;
  registro = numReg = 0;

  MODBUS_STATUS status;
  // Poco
  if ((status=leIdPoco(timeout,idPoco))) return status;
  // Funcao
  uint8_t funcao;
  if ((status=leFuncao(timeout,funcao))) {
    if (status==MODBUS_RESPOSTA_ERRO) {
      cerr << "Funcao de resposta de erro recebida no escravo: "
	   << funcao;
      limpaSocket();
    }
    return status;
  }

  // Endereco do registro inicial
  uint16_t regNorm;
  if ((status=leRegistroNormalizado(timeout,regNorm))) return status;
  registro = regNorm+registroInicial(funcao);
  // Dados
  leitura = (funcao <= 4);
  if (funcao==5 || funcao==6) { // Um unico registro
    numReg = 1;
  }
  else {                        // Varios registros
    if ((status=leNumeroRegistros(timeout,numReg))) return status;
  }
  if ((status=testaFaixaRegistros(regNorm,numReg))) {
    limpaSocket();
    return status;
  }
  if (!leitura) {
    int16_t lenDados = ((funcao==5 || funcao==15) ? 1 : 2)*numReg;
    if ((status=leDadosBuffer(timeout,lenDados))) return status;
  }
  return MODBUS_OK;
}

// Escravo envia dados para mestre
MODBUS_STATUS ModbusEscravo::write_data(uint8_t idPoco,
					uint16_t registro, uint16_t numReg,
					const uint8_t *dados)
{
  MODBUS_STATUS status;
  // ID
  if ((status=testaIdPoco(idPoco))) return status;
  // Dados
  if ((status=testaDados(dados))) return status;

  uint16_t regNorm;
  uint8_t funcao;
  int16_t lenDados;
  if ((status=converteRegistros(registro, numReg, true,
				regNorm, funcao, lenDados)))
    return status;

  // Poco e funcao
  if ((status=escreveIdFuncao(idPoco,funcao))) return status;

  // Numero de bytes
  if ((status=escreveTamanho(lenDados))) return status;

  // Dados
  return escreveDados(dados,lenDados);
}

/* *******************************************
   A classe MobusMestre
   ******************************************* */

// Construtores e destrutores
ModbusMestre::ModbusMestre(const char *nome, unsigned porta)
{
  pthread_mutex_init(&mutex,NULL);
  // Cria o socket
  sockId = ::socket(PF_INET, SOCK_DGRAM, 0);
  if (sockId<0) {
    cerr << "Nao foi possivel criar o socket\n";
    exit(1);
  }
  // Define o servidor
  struct sockaddr_in servidor;
  struct hostent *host;

  servidor.sin_family = AF_INET;
  servidor.sin_port = htons(porta);
  host = gethostbyname(nome);
  if( host==NULL || host->h_addrtype!=AF_INET ||
      host->h_length!=sizeof(struct in_addr) || host->h_addr_list[0]==NULL) {
    cerr << "Nao foi possivel determinar o host para se conectar\n";
    exit(1);
  }
  servidor.sin_addr = *(struct in_addr*)(host->h_addr_list[0]);
  // Conecta-se ao servidor
  if( ::connect(sockId,(struct sockaddr*)&servidor,sizeof(servidor)) == -1 ) {
    cerr << "Erro na conexao com o servidor\n";
    exit(1);
  }
}

// Funcoes privadas
ssize_t ModbusMestre::virtRecv(void *buf, size_t len, int flags)
{
  return ::recv(sockId, buf, len, flags);
}

MODBUS_STATUS ModbusMestre::leTamanho(int timeout, int16_t &lenDados)
{
  ssize_t numBytes;
  if ((numBytes=myRecv(timeout, &lenDados, sizeof(int16_t),
		       MSG_WAITALL)) != sizeof(int16_t)) {
    if (numBytes==0) return MODBUS_TIMEOUT;
    cerr << "Erro na leitura do comprimento dos dados\n";
    lenDados = 0;
    return MODBUS_ERRO_COMUNICACAO;
  }
  if (lenDados==0) {
    cerr << "Dados de comprimento nulo\n";
    limpaSocket();
    return MODBUS_ERRO_DADO;
  }
  return MODBUS_OK;
}

MODBUS_STATUS ModbusMestre::leDados(int timeout,
				    uint8_t *dados, int16_t &lenDados)
{
  ssize_t numBytes;
  if ((numBytes=myRecv(timeout, dados, lenDados,
		       MSG_WAITALL)) != lenDados) {
    if (numBytes==0) return MODBUS_TIMEOUT;
    cerr << "Erro na leitura dos dados\n";
    lenDados = 0;
    return MODBUS_ERRO_COMUNICACAO;
  }
  return MODBUS_OK;
}

MODBUS_STATUS ModbusMestre::escreveRegistros(uint16_t registro, uint16_t numReg)
{
  // Endereco do registro inicial
  if (send(sockId, &registro, sizeof(uint16_t), MSG_NOSIGNAL) != sizeof(uint16_t)) {
    cerr << "Erro na escrita do endereco do registro\n";
    return MODBUS_ERRO_COMUNICACAO;
  }
  // Numero de registros
  if (send(sockId, &numReg, sizeof(uint16_t), MSG_NOSIGNAL) != sizeof(uint16_t)) {
    cerr << "Erro na escrita do numero de registros\n";
    return MODBUS_ERRO_COMUNICACAO;
  }
  return MODBUS_OK;
}

// Funcoes publicas

// Mestre escreve dados no escravo
MODBUS_STATUS ModbusMestre::write_request(uint8_t idPoco,
					  uint16_t registro, uint16_t numReg,
					  const uint8_t *dados)
{
  MODBUS_STATUS status;
  // ID
  if ((status=testaIdPoco(idPoco))) return status;
  // Dados
  if ((status=testaDados(dados))) return status;

  uint16_t regNorm;
  uint8_t funcao;
  int16_t lenDados;
  if ((status=converteRegistros(registro, numReg, false,
				regNorm, funcao, lenDados)))
    return status;

  // Poco e funcao
  if ((status=escreveIdFuncao(idPoco,funcao))) return status;

  // Registros
  if ((status=escreveRegistros(regNorm,numReg))) return status;

  // Dados
  return escreveDados(dados,lenDados);
}

// Mestre pede dados ao escravo
MODBUS_STATUS ModbusMestre::request_data(uint8_t idPoco,
					 uint16_t registro, uint16_t numReg)
{
  MODBUS_STATUS status;
  // ID
  if ((status=testaIdPoco(idPoco))) return status;

  uint16_t regNorm;
  uint8_t funcao;
  int16_t lenDados;
  if ((status=converteRegistros(registro, numReg, true,
				regNorm, funcao, lenDados)))
    return status;

  // Poco e funcao
  if ((status=escreveIdFuncao(idPoco,funcao))) return status;

  // Registros
  return escreveRegistros(regNorm,numReg);
}

// Mestre recebe dados do escravo
MODBUS_STATUS ModbusMestre::receive_data(int &timeout, uint8_t &idPoco,
					 int16_t &lenDados, uint8_t *&dados)
{
  // Valores para o caso de retornar por erro
  idPoco = 0;
  lenDados = 0;

  MODBUS_STATUS status;
  // Dados
  if ((status=testaDados(dados))) return status;
  // Poco
  if ((status=leIdPoco(timeout,idPoco))) return status;

  // Funcao
  uint8_t funcao;
  if ((status=leFuncao(timeout,funcao))) return status;

  // Numero de bytes dos dados
  if ((status=leTamanho(timeout,lenDados))) return status;

  // Dados
  return leDados(timeout,dados,lenDados);
}

int ModbusMestre::requestReadAndWait(int to, uint8_t *data_,uint8_t wellId, uint16_t registerId, uint16_t &dataLen, int numberOdRegisters) {
	MODBUS_STATUS status;
	//printf("REQUISITANDO REGISTRADORES DO POCO %i",wellId);
	int16_t lenDados;
	pthread_mutex_lock(&mutex);
	status=request_data(wellId, registerId,numberOdRegisters);
	if( status != MODBUS_OK)
		goto out;
		
	status=receive_data(to, wellId, lenDados, data_);
	if( status != MODBUS_OK)
		goto out;
	
	dataLen = lenDados;
	goto out;
	
out:
	pthread_mutex_unlock(&mutex);
	return status;
}
